#include "Algorithm.h"
#include "CameraPara.h"
#include <string.h>
#include <stdlib.h>

#include <string>

typedef unsigned char uchar;

namespace movesense {

Algorithm::Algorithm(float *stereo_parameter, float *rgb_parameter)
{
	tP2_Stereo[0][0] = stereo_parameter[36];
	tP2_Stereo[1][1] = stereo_parameter[37];
	tP2_Stereo[0][2] = stereo_parameter[38];
	tP2_Stereo[1][2] = stereo_parameter[39];
	tP2_Stereo[0][3] = stereo_parameter[40]*stereo_parameter[36];

	M2_Rgb[0][0] = rgb_parameter[18];
	M2_Rgb[1][1] = rgb_parameter[19];
	M2_Rgb[0][2] = rgb_parameter[20];
	M2_Rgb[1][2] = rgb_parameter[21];

	for(int i = 0; i < 3 ; i++)
		for(int j = 0; j < 3; j++)
		{
			R_Rgb[i][j] = rgb_parameter[41 + i*3+j];
		}


    for(int i = 0; i < 3 ; i++)
    {
        T_Rgb[i][0] = rgb_parameter[50 + i];
	}

	fStereo = tP2_Stereo[0][0];
	cuStereo = tP2_Stereo[0][2];
	cvStereo = tP2_Stereo[1][2];
	fStereo1 = 1./fStereo;

	fxRGB = M2_Rgb[0][0];
	fyRGB = M2_Rgb[1][1];
	cuRGB = M2_Rgb[0][2];
	cvRGB = M2_Rgb[1][2];
	fxRGB1 = 1./fxRGB;
	fyRGB1 = 1./fyRGB;

	rRGB_00 = R_Rgb[0][0];
	rRGB_01 = R_Rgb[0][1];
	rRGB_02 = R_Rgb[0][2];
	rRGB_10 = R_Rgb[1][0];
	rRGB_11 = R_Rgb[1][1];
	rRGB_12 = R_Rgb[1][2];
	rRGB_20 = R_Rgb[2][0];
	rRGB_21 = R_Rgb[2][1];
	rRGB_22 = R_Rgb[2][2];

	tRGB_0 = T_Rgb[0][0];
	tRGB_1 = T_Rgb[1][0];
	tRGB_2 = T_Rgb[2][0];
	
	isDepthMap     = false;
	isRegistration = true;
}

Algorithm::~Algorithm()
{

}

void Algorithm::SpeckleCheck( unsigned short int* src, unsigned short int * dst, const int &width, const int &height, int speckleRange/* = 200*/, int cntSize/* = 200 */ )
{
	memcpy(dst, src, width*height*sizeof(unsigned short int));

	short newVal = 0;
	int _row, _col;

	int npixels = width*height;
	size_t bufSize = npixels*(int)(sizeof(int)* 2 + sizeof(int)+sizeof(uchar));
	uchar *_buf;
	_buf = new uchar[(int)bufSize];

	uchar *buf = _buf;
	int *labels = (int*)buf;//the label of points, noise or not

	int intSize = npixels * sizeof(int);
	buf += intSize;
	int *rcdPoints_x = (int*)buf;
	int *rcdPoints_y = (int*)buf + 1;

	buf += 2 * intSize;
	uchar* delePoints = (uchar*)buf; //label the noise pixel which should be delete

	int currLable = 0;
	memset(labels, 0, intSize);

	for (_row = 0; _row < height; ++_row)
	{
		int rowTmp = _row*width;
		unsigned short *dispPix = dst + rowTmp;
		int* lable_1 = labels + rowTmp; //label the group index of pixels

		for (_col = 0; _col < width; _col++)
		{
			if ((unsigned short)(dispPix[_col]) != newVal)   // not a bad disparity
			{
				if (lable_1[_col])     //each pixel has a label, which illustrates whether the pixel is a noise or not
				{
					if (delePoints[lable_1[_col]]) // small region, zero out disparity
					dispPix[_col] = (short)newVal; //zero the pixel
				}
				else
				{
					int *rp_x = rcdPoints_x;
					int *rp_y = rcdPoints_y;

					int currp_x = _col;
					int currp_y = _row;

					currLable++; // group label
					int count = 0;  // current region size
					lable_1[_col] = currLable;

					while (rp_x >= rcdPoints_x)
					{
						count++;
						int currRowTmp = currp_y*width;
						unsigned short int *dpp = &dst[currRowTmp + currp_x];

						unsigned short dp = *dpp; //the depth of the current pixel
						int* lable_2 = labels + currRowTmp + currp_x;

						//the pixel below the current location
						if (currp_y < height - 1 && !lable_2[+width] && (unsigned short)(dpp[+width]) != newVal && abs(dp - (unsigned short)(dpp[+width])) <= speckleRange)
						{
							lable_2[+width] = currLable;
							*rp_x = currp_x;
							*rp_y = currp_y + 1;
							rp_x += 2;
							rp_y += 2;
						}

						//the pixel above the current location
						if (currp_y >0 && !lable_2[-width] && (unsigned short)(dpp[-width]) != newVal && abs(dp - (unsigned short)(dpp[-width])) <= speckleRange)
						{
							lable_2[-width] = currLable;
							*rp_x = currp_x;
							*rp_y = currp_y - 1;
							rp_x += 2;
							rp_y += 2;
						}

						//the pixel behind the current location
						if (currp_x < width - 1 && !lable_2[+1] && (unsigned short)(dpp[+1]) != newVal && abs(dp - (unsigned short)(dpp[+1])) <= speckleRange)
						{
							lable_2[+1] = currLable;
							*rp_x = currp_x + 1;
							*rp_y = currp_y;
							rp_x += 2;
							rp_y += 2;
						}

						//the pixel before the current location
						if (currp_x > 0 && !lable_2[-1] && (unsigned short)(dpp[-1]) != newVal && abs(dp - (unsigned short)(dpp[-1])) <= speckleRange)
						{
							lable_2[-1] = currLable;
							*rp_x = currp_x - 1;
							*rp_y = currp_y;
							rp_x += 2;
							rp_y += 2;
						}
						rp_x -= 2;
						rp_y -= 2;
						currp_x = *rp_x;
						currp_y = *rp_y;
					}

					if (count <= cntSize)   // speckle region, the noises
					{
						delePoints[lable_1[_col]] = 1;   // small region label
						dispPix[_col] = (short)newVal;
					}
					else
					{
						delePoints[lable_1[_col]] = 0;   // large region label
					}
				}
			}
		}
	}
	delete _buf;
}

void Algorithm::GetPcd(const unsigned short int *src, const int &width, const int &height,  float *pcd)
{
	//点云保存在pcd的float型指针中，存储顺序是x, y, z
	memset(pcd, 0, width*height*3*sizeof(float));

	static bool fixRgbdLow = false;
	static float cuInv = 0,cvInv=0;
	static float fxInv = 0,fyInv=0;


	if( !fixRgbdLow )
	{
		cuInv = cuStereo;
		cvInv = cvStereo;
		fxInv = fStereo1;
		fyInv = fStereo1;
		fixRgbdLow = true;
	}

    if( isDepthMap )
    {	
		for ( int _row = 0; _row <height; _row++ )
		{
			int rowTmp = _row * width;
			for ( int _col = 0; _col < width; _col++ )
			{
				float z = src[rowTmp + _col];
				float x = (_col-cuInv)*z*fxInv;
				float y = (_row-cvInv)*z*fyInv;
		
				pcd[rowTmp*3 + _col*3] = x;
				pcd[rowTmp*3 + _col*3+1] = y;
				pcd[rowTmp*3 + _col*3+2] = z;
			}
		}
	}
	else
	{
		for ( int _row = 0; _row <height; _row++ )
		{
			int rowTmp = _row * width;
			for ( int _col = 0; _col < width; _col++ )
			{
				float fdisp = src[rowTmp + _col]/256.;
				int ndisp = (int)(fdisp);
				if ( !ndisp)
					continue;
			
				float z = abs(tP2_Stereo[0][3])/fdisp;
				float x = (_col-cuInv)*z*fxInv;
				float y = (_row-cvInv)*z*fyInv;
		
				pcd[rowTmp*3 + _col*3] = x;
				pcd[rowTmp*3 + _col*3+1] = y;
				pcd[rowTmp*3 + _col*3+2] = z;
			}
		}
	}
}

}
